#include "Temp.h"
#include "ObjHandler.h"

Cloth::Cloth(void)
{
}

Cloth::~Cloth(void)
{
}

void Cloth::InitCloth( char* filename )
{
	bu = bv = 5.0;
	k_stretch = 1;
	k_shear = 2;
	d_stretch = 2;
	d_shear = 0.002;
	

	OBJHandler mesh;
	mesh.LoadOBJascii(filename);

	for (int i=0;i<mesh.numVerts;i++)
	{
		Vertex* v=new Vertex;

		v->position=vec3f(mesh.verts[i].x,mesh.verts[i].y,mesh.verts[i].z);
		v->uv=vec2f(mesh.uvs[i].u,mesh.uvs[i].v);
		v->velocity=vec3f(0,0,0);
		v->force=vec3f(0,0,0);
		v->mass=0.0;
		v->index=i;

		vertices.push_back(v);
	}

	bool edge_dup;

	for (int i=0;i<mesh.numTris;i++)
	{
		Triangle* tri=new Triangle;

		tri->vertices[0]=vertices.at(mesh.tris[i].v1);
		tri->vertices[1]=vertices.at(mesh.tris[i].v2);
		tri->vertices[2]=vertices.at(mesh.tris[i].v3);

		tri->setArea();

		tri->vertices[0]->mass += 0.333 * DENSITY * tri->area;
		tri->vertices[1]->mass += 0.333 * DENSITY * tri->area;
		tri->vertices[2]->mass += 0.333 * DENSITY * tri->area;

		vec2f uv0, uv1, uv2;

		uv0 = tri->vertices[0]->uv;
		uv1 = tri->vertices[1]->uv;
		uv2 = tri->vertices[2]->uv;

		mat2f Muv;
		Muv[0][0] = uv1[0] - uv0[0];
		Muv[0][1] = uv2[0] - uv0[0];
		Muv[1][0] = uv1[1] - uv0[1];
		Muv[1][1] = uv2[1] - uv0[1];

		tri->uvInv = Muv.Inverse();

		/************************************************************************/
		/*							Edge List Init                              */
		/************************************************************************/

		//	e01

		edge_dup=false;

		for (int j=0;j<edges.size();j++)
		{
			Edge* e=edges.at(j);

			if ((tri->vertices[0]==e->vertices[0]&&tri->vertices[1]==e->vertices[1])||(tri->vertices[0]==e->vertices[1]&&tri->vertices[1]==e->vertices[0]))
			{
				edge_dup=true;

				e->boarder=1;
				e->triangles[1]=tri;

				tri->edges[0]=e;

				break;
			}
		}

		if (!edge_dup)
		{
			Edge* edge=new Edge;

			edge->vertices[0]=tri->vertices[0];
			edge->vertices[1]=tri->vertices[1];

			edge->boarder=0;
			edge->triangles[0]=tri;
			
			edges.push_back(edge);

			tri->edges[0]=edge;
		}

		//	e12

		edge_dup=false;

		for (int j=0;j<edges.size();j++)
		{
			Edge* e=edges.at(j);

			if ((tri->vertices[1]==e->vertices[0]&&tri->vertices[2]==e->vertices[1])||(tri->vertices[2]==e->vertices[0]&&tri->vertices[1]==e->vertices[1]))
			{
				edge_dup=true;

				e->boarder=1;
				e->triangles[1]=tri;

				tri->edges[1]=e;

				break;
			}
		}

		if (!edge_dup)
		{
			Edge* edge=new Edge;

			edge->vertices[0]=tri->vertices[1];
			edge->vertices[1]=tri->vertices[2];

			edge->boarder=0;
			edge->triangles[0]=tri;

			edges.push_back(edge);

			tri->edges[1]=edge;
		}

		//	e20

		edge_dup=false;

		for (int j=0;j<edges.size();j++)
		{
			Edge* e=edges.at(j);

			if ((tri->vertices[0]==e->vertices[0]&&tri->vertices[2]==e->vertices[1])||(tri->vertices[0]==e->vertices[1]&&tri->vertices[2]==e->vertices[0]))
			{
				edge_dup=true;

				e->boarder=1;
				e->triangles[1]=tri;

				tri->edges[2]=e;

				break;
			}
		}

		if (!edge_dup)
		{
			Edge* edge=new Edge;

			edge->vertices[0]=tri->vertices[2];
			edge->vertices[1]=tri->vertices[0];

			edge->boarder=0;
			edge->triangles[0]=tri;

			edges.push_back(edge);

			tri->edges[2]=edge;
		}

		/************************************************************************/
		/*								End                                     */
		/************************************************************************/

		triangles.push_back(tri);
	}

	// Stiffness Matrix Init

	solver.InitSparseStiffness(mesh.numVerts);

	for (int i=0;i<edges.size();i++)
	{
		Edge* edge=edges.at(i);

		Vertex *va, *vb;

		va = edge->vertices[0];
		vb = edge->vertices[1];

		// for vertex a

		StiffChain* sc = solver.sparse_M[va->index];

		if (sc->size()==0)
		{
			Stiff* stiffness = new Stiff;
			stiffness->index[0]=va->index;
			stiffness->index[1]=va->index;

			sc->push_back(stiffness);
		}

		Stiff* stiffness_a = new Stiff;
		stiffness_a->index[0]=va->index;
		stiffness_a->index[1]=vb->index;

		sc->push_back(stiffness_a);

		// for vertex b

		sc = solver.sparse_M[vb->index];

		if (sc->size()==0)
		{
			Stiff* stiffness = new Stiff;
			stiffness->index[0]=vb->index;
			stiffness->index[1]=vb->index;

			sc->push_back(stiffness);
		}

		Stiff* stiffness_b = new Stiff;
		stiffness_b->index[0]=vb->index;
		stiffness_b->index[1]=va->index;

		sc->push_back(stiffness_b);

	}

	fixed_A = vertices.at(FIXEDA)->position;
	fixed_B = vertices.at(FIXEDB)->position;

}

void Cloth::UpdateCloth( float dt )
{
	timestep=dt;

	UpdateForce();
	UpdateStiffness();
	TimeIntegration();
	ConstrainFixedPoint();
	
}

void Cloth::UpdateForce()
{
	ApplyExternalForce();
	ApplyStretchShearForce();
}

void Cloth::ApplyExternalForce()
{
	for (int i=0;i<vertices.size();i++)
	{
		vertices.at(i)->force=vec3f(0.0,-0.0098,0.01) * vertices.at(i)->mass;
	}
}

void Cloth::UpdateStiffness()
{
	ResetStiffness();
	AssemblyStiffness();
	//AssemblyTarget();
	AssemblyCoefficient();
}

void Cloth::TimeIntegration()
{
	//ForwardEuler();
	BackwardEuler();
}

void Cloth::ForwardEuler()
{
	for (int i=0;i<vertices.size();i++)
	{
		Vertex* v=vertices.at(i);

		v->velocity += timestep * v->force / v->mass;
		v->position += timestep * v->velocity;
	}
}

void Cloth::ConstrainFixedPoint()
{
	vertices.at(FIXEDA)->position = fixed_A;
	vertices.at(FIXEDB)->position = fixed_B;
	vertices.at(FIXEDA)->velocity.SetAll(0);
	vertices.at(FIXEDB)->velocity.SetAll(0);
}

void Cloth::ApplyStretchShearForce()
{
	vec3f dp1, dp2;	

	for (int i=0;i<triangles.size();i++)
	{
		Triangle* tri=triangles.at(i);

		Vertex* ca = tri->vertices[0];
		Vertex* cb = tri->vertices[1];
		Vertex* cc = tri->vertices[2];

		// delta_pi and delta_p2

		dp1 = tri->vertices[1]->position - tri->vertices[0]->position;
		dp2 = tri->vertices[2]->position - tri->vertices[0]->position;

		// wu and wv

		tri->wu = dp1 * tri->uvInv[0][0] + dp2 * tri->uvInv[1][0];
		tri->wv = dp1 * tri->uvInv[0][1] + dp2 * tri->uvInv[1][1];

		tri->wu_n2 = tri->wu.Length();
		tri->wv_n2 = tri->wv.Length();

		// derivative of wu and wv

		tri->wu_p[0] = mat3f::Identity();
		tri->wu_p[1] = mat3f::Identity();
		tri->wu_p[2] = mat3f::Identity();

		tri->wu_p[0] *= - tri->uvInv[0][0] - tri->uvInv[1][0];
		tri->wu_p[1] *= tri->uvInv[0][0];
		tri->wu_p[2] *= tri->uvInv[1][0];

		tri->wv_p[0] = mat3f::Identity();
		tri->wv_p[1] = mat3f::Identity();
		tri->wv_p[2] = mat3f::Identity();

		tri->wv_p[0] *= - tri->uvInv[0][1] - tri->uvInv[1][1];
		tri->wv_p[1] *= tri->uvInv[0][1];
		tri->wv_p[2] *= tri->uvInv[1][1];

		{	// stretch force

			vec3f m1, m2;
			float c1, c2;
			vec3f inf;

			for (int ii=0;ii<3;ii++)
			{
				m1 = tri->wu_p[ii] * tri->wu;
				m1 *= (tri->area / tri->wu_n2);

				m2 = tri->wv_p[ii] * tri->wv;
				m2 *= (tri->area / tri->wv_n2);

				c1 = k_stretch * (tri->area * (tri->wu_n2 - bu));
				c2 = k_stretch * (tri->area * (tri->wv_n2 - bv));

				//c1 += d_stretch * (tri->area * (wu_n2 - bu)) * (m1 * tri->vertices[0]->velocity);
				//c2 += d_stretch * (tri->area * (wv_n2 - bv)) * (m2 * tri->vertices[0]->velocity);

				inf = m1 * c1 + m2 * c2;
				inf *= -1;

				tri->vertices[ii]->force += inf;
			}			

		}

		/*{	// shear force

			vec3f m1, m2, m;
			vec3f inf;
			vec3f wu, wv;

			float c;

			wu = tri->wu;
			wv = tri->wv;

			wu.Normalize();
			wv.Normalize();

			for (int ii=0;ii<3;ii++)
			{
				m1 = tri->wu_p[ii] * wv;
				m2 = tri->wv_p[ii] * wu;
				m = tri->area * (m1 + m2);

				c = k_shear * tri->area * (wu * wv);// + d_shear * (m * tri->vertices[0]->velocity);

				inf = -1 * tri->area * c * m;

				tri->vertices[ii]->force += inf;
			}

		}*/




		
	}

}

void Cloth::ApplyBendForce()
{

}

void Cloth::ResetStiffness()
{
	for (int i=0;i<solver.scale;i++)
	{
		StiffChain* sc = solver.sparse_M[i];

		for (int j=0;j<sc->size();j++)
		{
			Stiff* sm = sc->at(j);

			sm->stiff.SetAll(0.0);
		}	
	}

}

mat3f Cloth::OuterProduct( mat3f a, mat3f b )
{
	mat3f result;

	result = a * b.Transpose();

	return result;
}

mat3f Cloth::OuterProduct( vec3f a, vec3f b )
{
	mat3f result;

	for (int i=0;i<3;i++)
	{
		for (int j=0;j<3;j++)
		{
			result[i][j] = a[i] * b[j];
		}
	}

	return result;
}


void Cloth::BackwardEuler()
{
	solver.SparseSolver();

	for (int i=0;i<vertices.size();i++)
	{
		Vertex* v=vertices.at(i);

		vec3f delta;

		delta[0] = solver.ls_x[i*3+0];
		delta[1] = solver.ls_x[i*3+1];
		delta[2] = solver.ls_x[i*3+2];

		v->velocity += delta;
		v->position += timestep * v->velocity;
	}
}

void Cloth::AssemblyStiffness()
{

	for (int t=0;t<triangles.size();t++)
	{
		Triangle* tri = triangles.at(t);

		mat3f KC[3][3];

		for (int i=0;i<3;i++)
		{
			for (int j=0;j<3;j++)
			{		
				mat3f K_sum;

				{ // stretch

					mat3f K;
					float cou, cov;


					cou = (tri->area * (tri->wu_n2 - bu));
					cov = (tri->area * (tri->wv_n2 - bv));

					K.SetAll(0.0);

					mat3f OuterU = OuterProduct(tri->wu, tri->wu) / (pow(tri->wu_n2, 2));
					mat3f OuterV = OuterProduct(tri->wv, tri->wv) / (pow(tri->wv_n2, 2));

					mat3f I = mat3f::Identity();

					mat3f CU = (I - OuterU) * (tri->area * tri->wu_p[i][0][0] * tri->wu_p[j][0][0] / tri->wu_n2);
					mat3f CV = (I - OuterV) * (tri->area * tri->wv_p[i][0][0] * tri->wv_p[j][0][0] / tri->wv_n2);

					K = CU * cou + CV * cov;

					// Second Term

					vec3f mui, mvi, muj, mvj;

					mui = tri->wu_p[i] * tri->wu;
					mui *= (tri->area / tri->wu_n2);

					mvi = tri->wv_p[i] * tri->wv;
					mvi *= (tri->area / tri->wv_n2);

					muj = tri->wu_p[j] * tri->wu;
					muj *= (tri->area / tri->wu_n2);

					mvj = tri->wv_p[j] * tri->wv;
					mvj *= (tri->area / tri->wv_n2);

					K += (OuterProduct(muj, mui) + OuterProduct(mvj, mvi));

					K *= (-1 * k_stretch);

					K_sum = K;

				}



				/*{ // shear

					vec3f wu, wv;
					vec3f mui, muj, mvi, mvj, mi, mj;
					mat3f K;

					float c;

					wu = tri->wu;
					wv = tri->wv;

					wu.Normalize();
					wv.Normalize();

					mui = tri->wu_p[i] * wv;
					mvi = tri->wv_p[i] * wu;

					mi = tri->area * (mui + mvi);

					c = tri->area * (wu * wv);

					muj = tri->wu_p[j] * wv;
					mvj = tri->wv_p[j] * wu;

					mj = tri->area * (muj + mvj);

					K = OuterProduct(mj, mi);

					mat3f sub_K;

					sub_K = (tri->wv_p[j] * tri->wu_p[i].Transpose() + tri->wu_p[j] * tri->wv_p[i].Transpose()) * tri->area * c;

					K += sub_K;

					K *= (-1 * k_shear);

					K_sum += K;

				}*/


				Vertex* vert = tri->vertices[i];
				Vertex* neigh = tri->vertices[j];


				StiffChain* sc = solver.sparse_M[vert->index];

				for (int s=0;s<sc->size();s++)
				{
					Stiff* sm = sc->at(s);


					if (sm->index[0]==vert->index&&sm->index[1]==neigh->index)
					{
						sm->stiff += K_sum;
					}
				}

				

				



			}
		}



	}

	
	
}

void Cloth::AssemblyTarget()
{
	for (int i=0;i<vertices.size();i++)
	{
		vec3f cache(0,0,0);
		Vertex* v = vertices.at(i);

		StiffChain* sc = solver.sparse_M[vertices.at(i)->index];

		for (int j=0;j<sc->size();j++)
		{
			Stiff* sm = sc->at(j);

			Vertex* vert = vertices.at(sm->index[1]);

			cache += sm->stiff * vert->velocity;
		}

		cache = cache * timestep + v->force;
		cache *= timestep;

		solver.ls_b[i*3+0] = cache[0];
		solver.ls_b[i*3+1] = cache[1];
		solver.ls_b[i*3+2] = cache[2];		
	}

	for (int i=0;i<vertices.size();i++)
	{
		StiffChain* sc = solver.sparse_M[vertices.at(i)->index];
		Vertex* v = vertices.at(i);

		int offset_a, offset_b, offset_c;

		offset_a = offset_b = offset_c = 0;

		for (int j=0;j<sc->size();j++)
		{
			Stiff* sm = sc->at(j);

			mat3f cache = sm->stiff * (-1 * pow(timestep, 2));			

			for (int x=0;x<3;x++)
			{
				for (int y=0;y<3;y++)
				{
					int p, q;

					p = sm->index[0] * 3 + x;
					q = sm->index[1] * 3 + y;

					if (p==q)
					{
						cache[x][y] += v->mass;
					}

					if (x==0)
					{
						solver.ls_A[p][offset_a] = cache[x][y];
						solver.ls_index[p][offset_a] = q;
						offset_a++;
					}else if (x==1)
					{
						solver.ls_A[p][offset_b] = cache[x][y];
						solver.ls_index[p][offset_b] = q;
						offset_b++;
					}else if (x==2)
					{
						solver.ls_A[p][offset_c] = cache[x][y];
						solver.ls_index[p][offset_c] = q;
						offset_c++;
					}
				}
			}
		}
	}
}

void Cloth::AssemblyCoefficient()
{
	for (int i=0;i<vertices.size();i++)
	{
		vec3f cache(0,0,0);
		Vertex* v = vertices.at(i);

		StiffChain* sc = solver.sparse_M[vertices.at(i)->index];

		for (int j=0;j<sc->size();j++)
		{
			Stiff* sm = sc->at(j);

			Vertex* vert = vertices.at(sm->index[1]);

			cache += sm->stiff * vert->velocity;
		}

		cache = cache * timestep + v->force;
		cache *= timestep;

		solver.ls_b[i*3+0] = cache[0];
		solver.ls_b[i*3+1] = cache[1];
		solver.ls_b[i*3+2] = cache[2];		
	}

	for (int i=0;i<vertices.size();i++)
	{
		StiffChain* sc = solver.sparse_M[vertices.at(i)->index];
		Vertex* v = vertices.at(i);

		int offset_a, offset_b, offset_c;

		offset_a = offset_b = offset_c = 0;

		for (int j=0;j<sc->size();j++)
		{
			Stiff* sm = sc->at(j);

			mat3f cache = sm->stiff * (-1 * pow(timestep, 2));			

			for (int x=0;x<3;x++)
			{
				for (int y=0;y<3;y++)
				{
					int p, q;

					p = sm->index[0] * 3 + x;
					q = sm->index[1] * 3 + y;

					if (p==q)
					{
						cache[x][y] += v->mass;
					}

					if (x==0)
					{
						solver.ls_A[p][offset_a] = cache[x][y];
						solver.ls_index[p][offset_a] = q;
						offset_a++;
					}else if (x==1)
					{
						solver.ls_A[p][offset_b] = cache[x][y];
						solver.ls_index[p][offset_b] = q;
						offset_b++;
					}else if (x==2)
					{
						solver.ls_A[p][offset_c] = cache[x][y];
						solver.ls_index[p][offset_c] = q;
						offset_c++;
					}
				}
			}
		}
	}
}